#include <math.h>
#include <stdlib.h>
#include "fdecl.h"
#include "misc.h"

#define ITMAX 200
#define EPS 1.0e-10

void bilinear_patch
  (double s, double t, double h, double k,
   vector *p00, vector *p01, vector *p10, vector *p11,
   vector *f)
{
  double hs = h - s, kt = k - t, hk = h * k;
  vector p00p, p10p, p01p, p11p;

  vscale1(hs * kt / hk, *p00, p00p);
  vscale1(s  * kt / hk, *p10, p10p);
  vscale1(s  *  t / hk, *p11, p11p);
  vscale1(hs *  t / hk, *p01, p01p);
  vplus(p00p, p10p, p00p);
  vplus(p11p, p01p, p11p);
  vplus(p00p, p11p, *f);
}


double squared_distance_to_bilinear_patch
  (vector *p, 
   double s, double t, double h, double k,
   vector *p00, vector *p01, vector *p10, vector *p11)
{
  vector fst;
  bilinear_patch(s, t, h, k, p00, p01, p10, p11, &fst);
  vminus(fst, *p, fst);
  return(fst.x * fst.x + fst.y * fst.y + fst.z * fst.z);
}


void grad_squared_distance_to_bilinear_patch
    (vector *p, 
     double s, double t, double h, double k,
     vector *p00, vector *p01, vector *p10, vector *p11,
     double *fu, double *fv)
{
  double hs = h - s, kt = k - t, hk = h * k;
  vector p00p, p10p, p01p, p11p, fst;

  bilinear_patch(s, t, h, k, p00, p01, p10, p11, &fst);
  vminus(fst, *p, fst);

  vscale1(- kt / hk, *p00, p00p);
  vscale1(  kt / hk, *p10, p10p);
  vscale1(   t / hk, *p11, p11p);
  vscale1(-  t / hk, *p01, p01p);
  vplus(p00p, p10p, p00p);
  vplus(p11p, p01p, p11p);
  vplus(p00p, p11p, p11p);
  *fu = 2.0 * vdot(p11p, fst);

  vscale1(- hs / hk, *p00, p00p);
  vscale1(-  s / hk, *p10, p10p);
  vscale1(   s / hk, *p11, p11p);
  vscale1(  hs / hk, *p01, p01p);
  vplus(p00p, p10p, p00p);
  vplus(p11p, p01p, p11p);
  vplus(p00p, p11p, p11p);
  *fv = 2.0 * vdot(p11p, fst);
}



void compute_nearest_point_on_bilinear_patch
  (vector *p,
   double *u, double *v,
   double h, double k,
   vector *p00, vector *p01, vector *p10, vector *p11,
   double ftol, int *iter, 
   double *fret)
{
  /* 
    more efficient version of quasi-newton method (Numerical Recipes pg, 327)
    specialized to distance minimization on a bicubic patch defined in (u, v) 
    space by 4 corner nodes. initial iteration uses cubic polynomial fit line
    search in 0<=s<=1. (see also siggraph 86, pg 279)
    */

  double huu = 1.0, hvv = 1.0, huv = 0.0;
  double u0, v0, u1, v1, f0, f1, fu0, fv0, fu1, fv1, df0, df1, a, b, ss;
  double fp, fu, fv, dfu, dfv, hdfu, hdfv, xiu, xiv, fae, fad, fac;
  int its;

  u0 = *u; v0 = *v;

  f0 = squared_distance_to_bilinear_patch(p, u0, v0, h, k, p00, p01, p10, p11);
  grad_squared_distance_to_bilinear_patch(p, u0, v0, h, k, p00, p01, p10, p11,
					  &fu0, &fv0);
  df0 = -(double)sqrtf((float)(fu0 * fu0 + fv0 * fv0));
  u1 = u0 - fu0; v1 = v0 - fv0;
  f1 = squared_distance_to_bilinear_patch(p, u1, v1, h, k, p00, p01, p10, p11);
  grad_squared_distance_to_bilinear_patch(p, u1, v1, h, k, p00, p01, p10, p11,
					  &fu1, &fv1);
  df1 = -(fu1 * fu0  + fv1 * fv0) / (-df0);

  /* min of cubic fit (Luenberger-73, pg 142) */

  a = df0 + df1 + 3.0 * (f0 - f1);
  b = (double)sqrtf((float)(a * a - df0 * df1));
  ss = 1.0 - (df1 + b - a) / (df1 - df0 + b + b);

  *u = u0  - ss * fu0; *v = v0 - ss * fv0;
  fp = squared_distance_to_bilinear_patch(p, *u, *v, h, k, 
					  p00, p01, p10, p11);
  grad_squared_distance_to_bilinear_patch(p, *u, *v, h, k, 
					  p00, p01, p10, p11, &fu, &fv);
  xiu = -fu; xiv = -fv;

  for (its = 1; its <= ITMAX; its++) {
    *iter = its;
    *u += xiu; *v += xiv;
    *fret = squared_distance_to_bilinear_patch(p, *u, *v, h, k, 
					       p00, p01, p10, p11);
    if (2.0 * fabs(*fret - fp) <= ftol * (fabs(*fret) + fabs(fp) + EPS))
      return;
    
    fp = *fret;
    dfu = fu; dfv = fv;
    grad_squared_distance_to_bilinear_patch(p, *u, *v, h, k, 
					    p00, p01, p10, p11, &fu, &fv);
    dfu = fu - dfu; dfv = fv - dfv;
    hdfu = huu * dfu + huv * dfv; hdfv = huv * dfu + hvv * dfv;
    
    fac = dfu * xiu + dfv * xiv;
    fae = dfu * hdfu + dfv * hdfv;
    fac = 1.0 / fac; fad = 1.0 / fae;
    dfu = fac * xiu - fad * hdfu; dfv = fac * xiv - fad * hdfv;
    
    huu += fac * xiu * xiu - fad * hdfu * hdfu + fae * dfu * dfu;
    huv += fac * xiu * xiv - fad * hdfu * hdfv + fae * dfu * dfv;
    hvv += fac * xiv * xiv - fad * hdfv * hdfv + fae * dfv * dfv;

    xiu = -(huu * fu + huv * fv); xiv = -(huv * fu + hvv * fv);
  }
  printf("%d iterations in `quasi_newton'\n", its);
  exit(1);
}


void superquadric
  (double u, double v, 
   double r1, double r2, double r3, double e1, double e2,
   vector *f)
{
  double su = sinf((float)u), cu = cosf((float)u); 
  double cv = cosf((float)v), sv = sinf((float)v);

  f->x = r1 * Sign(cu) * pow(fabs(cu), e1) * Sign(cv) * pow(fabs(cv), e2);
  f->y = r2 * Sign(cu) * pow(fabs(cu), e1) * Sign(sv) * pow(fabs(sv), e2);
  f->z = r3 * Sign(su) * pow(fabs(su), e1);
}


double squared_distance_to_superquadric
  (vector *p, double u, double v,
   double r1, double r2, double r3, double e1, double e2)
{
  vector f;
  superquadric(u, v, r1, r2, r3, e1, e2, &f);
  vminus(f, *p, f);
  return(f.x * f.x + f.y * f.y + f.z * f.z);
}


void superquadric_grad_hack
  (double u, double v, 
   double r1, double r2, double r3, double e1, double e2,
   vector *f)
{
  double su = sinf((float)u), cu = cosf((float)u); 
  double cv = cosf((float)v), sv = sinf((float)v);

  f->x = r1 * 
    ((e1 == 0.0) ? 1.0 : (Sign(cu) * pow(fabs(cu), e1))) * 
      ((e2 == 0.0) ? 1.0 : (Sign(cv) * pow(fabs(cv), e2)));
  f->y = r2 *
    ((e1 == 0.0) ? 1.0 : (Sign(cu) * pow(fabs(cu), e1))) * 
      ((e2 == 0.0) ? 1.0 : (Sign(sv) * pow(fabs(sv), e2)));
  f->z = r3 * 
    ((e1 == 0.0) ? 1.0 : (Sign(su) * pow(fabs(su), e1)));
}


void grad_squared_distance_to_superquadric
  (vector *p, 
   double u, double v,
   double r1, double r2, double r3, double e1, double e2,
   double *fu, double *fv)
{
  double su = sinf((float)u), cu = cosf((float)u); 
  double cv = cosf((float)v), sv = sinf((float)v);
  vector f, s;

  superquadric_grad_hack(u, v, r1, r2, r3, e1, e2, &f);
  vminus(f, *p, f);

  superquadric_grad_hack(u, v, r1, r2, r3, (e1 - 1.0), e2, &s);
  s.x *= -e1 * su; s.y *= -e1 * su; s.z *= e1 * cu;
  *fu = 2.0 * vdot(s, f);

  superquadric_grad_hack(u, v, r1, r2, r3, e1, (e2 - 1.0), &s);
  s.x *= -e2 * sv; s.y *= e2 * cv; s.z = 0.0;
  *fv = 2.0 * vdot(s, f);
}


#define RADIAN_LIMIT 3.14159/8.0

#define limit_grad_squared_distance(fu, fv)\
  fu = Limit(fu, -RADIAN_LIMIT, RADIAN_LIMIT);\
  fv = Limit(fv, -RADIAN_LIMIT, RADIAN_LIMIT)


void compute_nearest_point_on_superquadric
  (vector *p,
   double *u, double *v,
   double r1, double r2, double r3, double e1, double e2,
   double ftol, int *iter, 
   double *fret)
{
  /* 
    more efficient version of quasi-newton method (Numerical Recipes pg, 327)
    specialized to distance minimization on a bicubic patch defined in (u, v) 
    space by 4 corner nodes. initial iteration uses cubic polynomial fit line
    search in 0<=s<=1. (see also siggraph 86, pg 279)
    */

  double huu = 1.0, hvv = 1.0, huv = 0.0;
  double u0, v0, u1, v1, f0, f1, fu0, fv0, fu1, fv1, df0, df1, a, b, ss;
  double fp, fu, fv, dfu, dfv, hdfu, hdfv, xiu, xiv, fae, fad, fac;
  int its;

  u0 = *u; v0 = *v;

  f0 = squared_distance_to_superquadric(p, u0, v0, r1, r2, r3, e1, e2);
  grad_squared_distance_to_superquadric(p, u0, v0, r1, r2, r3, e1, e2,
					&fu0, &fv0);
  limit_grad_squared_distance(fu0, fv0);
  df0 = -(double)sqrtf((float)(fu0 * fu0 + fv0 * fv0));
  u1 = u0 - fu0; v1 = v0 - fv0;
  f1 = squared_distance_to_superquadric(p, u1, v1, r1, r2, r3, e1, e2);
  grad_squared_distance_to_superquadric(p, u1, v1, r1, r2, r3, e1, e2,
					&fu1, &fv1);
  limit_grad_squared_distance(fu1, fv1);
  df1 = -(fu1 * fu0  + fv1 * fv0) / (-df0);

  /* min of cubic fit (Luenberger-73, pg 142) */

  a = df0 + df1 + 3.0 * (f0 - f1);
  b = (double)sqrtf((float)(a * a - df0 * df1));
  ss = 1.0 - (df1 + b - a) / (df1 - df0 + b + b);

  *u = u0  - ss * fu0; *v = v0 - ss * fv0;
  fp = squared_distance_to_superquadric(p, *u, *v, r1, r2, r3, e1, e2);
  grad_squared_distance_to_superquadric(p, *u, *v, r1, r2, r3, e1, e2, 
					&fu, &fv);
  limit_grad_squared_distance(fu, fv);
  xiu = -fu; xiv = -fv;

  for (its = 1; its <= ITMAX; its++) {
    *iter = its;
    *u += xiu; *v += xiv;
    *fret = squared_distance_to_superquadric(p, *u, *v, r1, r2, r3, e1, e2);
    if (2.0 * fabs(*fret - fp) <= ftol * (fabs(*fret) + fabs(fp) + EPS))
      return;
    
    fp = *fret;
    dfu = fu; dfv = fv;
    grad_squared_distance_to_superquadric(p, *u, *v, r1, r2, r3, e1, e2, 
					  &fu, &fv);
    limit_grad_squared_distance(fu, fv);
    dfu = fu - dfu; dfv = fv - dfv;
    hdfu = huu * dfu + huv * dfv; hdfv = huv * dfu + hvv * dfv;
    
    fac = dfu * xiu + dfv * xiv;
    fae = dfu * hdfu + dfv * hdfv;
    fac = 1.0 / fac; fad = 1.0 / fae;
    dfu = fac * xiu - fad * hdfu; dfv = fac * xiv - fad * hdfv;
    
    huu += fac * xiu * xiu - fad * hdfu * hdfu + fae * dfu * dfu;
    huv += fac * xiu * xiv - fad * hdfu * hdfv + fae * dfu * dfv;
    hvv += fac * xiv * xiv - fad * hdfv * hdfv + fae * dfv * dfv;

    xiu = -(huu * fu + huv * fv); xiv = -(huv * fu + hvv * fv);
  }
  printf("%d iterations in `quasi_newton'\n", its);
  exit(1);
}


void compute_nearest_point_on_sphere
  (vector *p,
   double *u, double *v,
   double radius,
   double *fret)
{
  double pi = 3.141592653589;
  double x = p->x, y = p->y, z = p->z;
  
  if (x == 0.0 && y == 0.0) {
    *u = (z < 0.0) ? -pi/2.0 : pi/2.0;
    *v = 0.0;
  } else {
    *u = atan2(z, sqrt(x * x + y * y));
    *v = (fabs(y) < 0.000001 && x < 0.0) ? -pi : atan2(p->y, p->x);
  }
  *fret = vlength(*p) - radius;
}

